/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import java.util.Vector;

/**
 * Autonomous mode
 * @author robert
 */
public class PhoenixAutonomous {
    PhoenixDrive drive;
    PhoenixLifter lifter;
    PhoenixSensors sensors;
    PhoenixArm arm;
    long startTime;
    public Stage currentStage = Stage.kStraight;
    public Direction lastDirection;
    public AutonomousMode currentMode;
    public Vector debugMessages = new Vector(0);

    /**
     * constructer taking drive, lifter and light sensor classes
     * @param robotDrive
     * @param lifter
     * @param sensors
     */
    public PhoenixAutonomous (PhoenixDrive drive, PhoenixLifter lifter, 
            PhoenixSensors sensors, PhoenixArm arm) {
        this.drive = drive;
        this.lifter = lifter;
        this.sensors = sensors;
        this.arm = arm;
    }

    public void addMessage(String message) {
        debugMessages.addElement(message);
    }

    /**
     * starts autonomous
     */
    public void start() {
        startTime = System.currentTimeMillis();
        drive.encoderStart();
        //lifter.startPID();
        unalignedDirection = null;
    }

    /**
     * stops autonomous
     */
    public void stop() {
        lifter.stopPID();
    }

    public AutonomousMode getMode() {
        return currentMode;
    }

    /**
     * updates the current stage
     * @param mode
     */
    public void updateStage(AutonomousMode mode) {
        if(currentStage == Stage.kFinished)
            return;

        switch(mode.value) {
            case AutonomousMode.kDisabled_val:
                addMessage("mode: disabled");
                break;
            case AutonomousMode.kForkLeft_val:
                addMessage("mode: fork left");
                updateFork(); break;
            case AutonomousMode.kForkRight_val:
                addMessage("mode: fork right");
                updateFork(); break;
            case AutonomousMode.kStraight_val:
                addMessage("mode: straight");
                updateStraight(); break;
        }
        currentMode = mode;
    }

    Direction unalignedDirection = null;

    /**
     * drive motors for autnomous
     * @param movementScale scale for movement
     */
    public void run(double movementScale) {
        // drive logic
        if (currentStage != null && currentStage != Stage.kFinished) {
            followLine(movementScale);
        } else {
            drive.tankDrive(0, 0);
        }

        updateLifter();
    }

    /**
     * updates stages for autonomous on straight path
     */
    private void updateStraight() {

        /* NOTE: values denote end encoder values of each mode relative to
         * startPID of autonomous.
         */

        // constants for encoder values
        // straight path
        final int endStageStraight = 4000;
        // preparing to stop
        final int endStageFinishing = 6000;

        // left encoder values used because right is broken
        if (drive.getEncoderLeft() > endStageFinishing ||
                (currentStage == Stage.kStopping &&
                sensors.getDirection() == Direction.kArrived)) {
            currentStage = Stage.kFinished;
            addMessage("Stage: finished");
        } else if (drive.getEncoderLeft() > 0
                && drive.getEncoderLeft() <= endStageStraight) {
            currentStage = Stage.kStraight;
            addMessage("Stage: straight");
        } else if (drive.getEncoderLeft() > endStageStraight
                && drive.getEncoderLeft() <= endStageFinishing) {
            currentStage = Stage.kStopping;
            addMessage("Stage: stopping");
        }
    }

    /**
     * updates stages for autonomous on forked path
     */
    private void updateFork() {
        /* NOTE: values denote end encoder values of each mode relative to
         * startPID of autonomous.
         */

        //TODO: calibrate
        // initial straight stint
        int endStageStraightFirst = 1000;
        // first right turn
        int endStageTurnFirst = 2000;
        // second straight part
        int endStageStraightSecond = 3000;
        // second right turn
        int endStageTurnSecond = 4000;
        // approaching stopping T
        int endStageStopping = 5000;

        if (currentStage == Stage.kStopping &&
                sensors.getDirection() == Direction.kArrived) {
            currentStage = Stage.kStraightenOut;
        } else if (isStraightened) {
            currentStage = Stage.kFinished;
        } else if (drive.getEncoderLeft() > 0
                && drive.getEncoderLeft() <= endStageStraightFirst) {
            currentStage = Stage.kStraight;
        } else if (drive.getEncoderLeft() > endStageStraightFirst
                && drive.getEncoderLeft() <= endStageTurnFirst) {
            currentStage = Stage.kRightTurn;
        } else if (drive.getEncoderLeft() > endStageTurnFirst
                && drive.getEncoderLeft() <= endStageStraightSecond) {
            currentStage = Stage.kStraight;
        } else if (drive.getEncoderLeft() > endStageStraightSecond
                && drive.getEncoderLeft() <= endStageTurnSecond) {
            currentStage = Stage.kLeftTurn;
        } else if (drive.getEncoderLeft() > endStageTurnSecond
                && drive.getEncoderLeft() <= endStageStopping) {
            currentStage = Stage.kStopping;
        }
    }

    /**
     * gets the current running time of autonomous
     * @return time enabled for
     */
    public long getTimeEnabled() {
        return System.currentTimeMillis() - startTime;
    }

    public boolean isStraightened = false;

    /**
     * align robot to T at end of line
     * @deprecated only one encoder working
     * @param scale motor scaling
     * @param direction direction the robot should turn
     */
    private void alignToT(double scale, Direction direction) {
        double turnScale =  scaleToTurn(scale);

        if(direction == Direction.kLeft) {
            // if robot is still unaligned, turn left.
            if(drive.getEncoderLeft() >= drive.getEncoderRight()) {
                drive.tankDrive(-.2 * turnScale, 1 * turnScale);
            } else {
                isStraightened = true;
            }
        } else if (direction == Direction.kRight) {
            // if robot is still unaligned, turn right.
            if(drive.getEncoderLeft() < drive.getEncoderRight()) {
                drive.tankDrive(1 * turnScale, -.2 * turnScale);
            } else {
                isStraightened = true;
            }
        }
    }

    public void autonomousTest() {
        if(getTimeEnabled() < 1000) {
            drive.tankDrive(.2, .2);
        }
    }

    /**
     * changes input scaling value to better suit turning
     * @param scale input scale
     * @return turn scaling
     */
    public double scaleToTurn(double scale) {
        return (scale / 2) + .5;
    }

    /**
     * follows line in autonomous
     * @param scale motor scale
     */
    public void followLine(double scale) {
        double turnScale = scaleToTurn(scale);

        if (sensors.getDirection() == Direction.kStraight) {
            drive.tankDrive(1 * scale, 1 * scale);
            lastDirection = Direction.kStraight;
            addMessage("line following: straight");
        }
        if (sensors.getDirection() == Direction.kRight) {
            drive.tankDrive(-0.2 * turnScale, 1 * turnScale);
            lastDirection = Direction.kRight;
            addMessage("line following: left");
        }
        if (sensors.getDirection() == Direction.kLeft) {
            drive.tankDrive(1 * turnScale, -0.2 * turnScale);
            lastDirection = Direction.kLeft;
            addMessage("line following: right");
        }
        if (sensors.getDirection() == Direction.kLost) {
            // NOTE: remove and uncomment findLine if tests are positive
            currentStage = Stage.kFinished;
            //findLine(scale);
        }
    }

    /**
     * when robot has gone off line, find it again
     * @param scale scale for motors
     */
    private void findLine(double scale) {
        double turnScale = scaleToTurn(scale);

        switch(currentStage.value) {
            // if we're in left or right turn mode, only find line in that
            // direction
            case Stage.kRightTurn_val:
                addMessage("lost, searching for line to the right");
                drive.tankDrive(1 * turnScale, -1 * turnScale); break;
            case Stage.kLeftTurn_val:
                addMessage("lost, searching for line to the left");
                drive.tankDrive(-1 * turnScale, 1 * turnScale); break;

            // if not, find line in direction of last
            default:
                switch(lastDirection.value) {
                    // if we're just seeking the line
                    case Direction.kLeft_val:
                        addMessage("lost, searching for line to the right");
                        drive.tankDrive(1 * turnScale, -.5 * turnScale); break;
                    case Direction.kRight_val:
                        addMessage("lost, searching for line to the left");
                        drive.tankDrive(-.5 * turnScale, 1 * turnScale); break;
                } break;
        }

        /*
        if (currentStage == Stage.kRightTurn) {
            drive.tankDrive(1 * turnScale, -1 * turnScale);
        } else if (currentStage == Stage.kLeftTurn) {
            drive.tankDrive(-1 * turnScale, 1 * turnScale);
        } else if (currentStage == Stage.kStraight) {
            if (lastDirection == Direction.kLeft) {
                drive.tankDrive(1 * turnScale, -1 * turnScale);
            } else if (lastDirection == Direction.kRight) {
                drive.tankDrive(-1 * turnScale, 1 * turnScale);
            }
        }*/
    }

    private long lowerLifterStartTime = 0;

    /**
     * updates lifter and arm for autonomous
     */
    public void updateLifter() {
        // time in milliseconds that lifter will lower
        int lowerLifterTime = 500;

        // lifter logic
        if(currentStage != Stage.kFinished) {
            // raise lifter and close claw
            addMessage("Raising arm");
            arm.setClawOpen(false);

            lifter.setLifterPowerRaw(lifter.getLimitTop() ? 0 : 0.8);
            
        } else {
            // lower lifter and open claw
            addMessage("Lowering arm");
            arm.setClawOpen(true);
            if(lowerLifterStartTime == 0) {
                lowerLifterStartTime = System.currentTimeMillis();
            }
            if(System.currentTimeMillis() < lowerLifterStartTime + lowerLifterTime) {
                lifter.setLifterPowerRaw(-.8);
                // may consider backing up at this point
            } else {
                lifter.setLifterPowerRaw(0);
            }
        }
    }

    /**
     * mode for autonomous operation
     */
    public static class AutonomousMode {
        public final int value;
        private AutonomousMode (int value) {
            this.value = value;
        }

        static final int kStraight_val = 0;
        static final int kForkLeft_val = 1;
        static final int kForkRight_val = 2;
        static final int kDisabled_val = 3;

        /**
         * autonomous mode for straight lines
         */
        public static final AutonomousMode kStraight =
                new AutonomousMode(kStraight_val);

        /**
         * autonomous mode for left fork
         */
        public static final AutonomousMode kForkLeft =
                new AutonomousMode(kForkLeft_val);

        /**
         * autonomous mode for right fork
         */
        public static final AutonomousMode kForkRight =
                new AutonomousMode(kForkRight_val);

        /**
         * autonomous is disabled
         */
        public static final AutonomousMode kDisabled =
                new AutonomousMode(kDisabled_val);
    }
}
